import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import interp1d

# 读取数据
file_name = "E:/crp_linux_pc/build_windows/bin/bin/Debug/pos/RobotPosture3.txt"
file_name1 = "E:/crp_linux_pc/build_windows/bin/bin/Debug/pos/RobotPosture4.txt"

data = np.loadtxt(file_name, delimiter=',')
data1 = np.loadtxt(file_name1, delimiter=',')

x = data[:,0]
y = data[:,1]
z = data[:,2]

x1 = data1[:,0]
y1 = data1[:,1]
z1 = data1[:,2]

# 计算速度
dt = 0.002
vx = np.diff(x) / dt
vy = np.diff(y) / dt
vz = np.diff(z) / dt

vx1 = np.diff(x1) / dt
vy1 = np.diff(y1) / dt
vz1 = np.diff(z1) / dt

# 假设我们选择最后5个点作为第一段的减速段，前5个点作为第二段的加速段
blend_length = 5
end_index = len(vx) - blend_length
start_index = blend_length

# 提取需要进行blending的部分
x_blend = x[end_index:end_index+blend_length]
y_blend = y[end_index:end_index+blend_length]
z_blend = z[end_index:end_index+blend_length]

x1_blend = x1[start_index:start_index+blend_length]
y1_blend = y1[start_index:start_index+blend_length]
z1_blend = z1[start_index:start_index+blend_length]

# 使用多项式插值来进行blending
t = np.linspace(0, 1, blend_length)
tx = interp1d(t, x_blend, kind='cubic')(np.linspace(0, 1, blend_length*2))
ty = interp1d(t, y_blend, kind='cubic')(np.linspace(0, 1, blend_length*2))
tz = interp1d(t, z_blend, kind='cubic')(np.linspace(0, 1, blend_length*2))

tx1 = interp1d(t, x1_blend[::-1], kind='cubic')(np.linspace(0, 1, blend_length*2))
ty1 = interp1d(t, y1_blend[::-1], kind='cubic')(np.linspace(0, 1, blend_length*2))
tz1 = interp1d(t, z1_blend[::-1], kind='cubic')(np.linspace(0, 1, blend_length*2))

# 混合轨迹
x_blended = np.concatenate((x[:end_index], tx[:-1], x1_blend[-1:]))
y_blended = np.concatenate((y[:end_index], ty[:-1], y1_blend[-1:]))
z_blended = np.concatenate((z[:end_index], tz[:-1], z1_blend[-1:]))

# 混合速度
vx_blended = np.concatenate((vx[:end_index-1], (tx[1:] - tx[:-1]) / dt, vx1[start_index:]))
vy_blended = np.concatenate((vy[:end_index-1], (ty[1:] - ty[:-1]) / dt, vy1[start_index:]))
vz_blended = np.concatenate((vz[:end_index-1], (tz[1:] - tz[:-1]) / dt, vz1[start_index:]))

# 绘制轨迹曲线
plt.figure(figsize=(12, 6))
plt.subplot(1, 2, 1)
plt.plot(x_blended, label='X', color='r')
plt.plot(y_blended, label='Y', color='g')
plt.plot(z_blended, label='Z', color='b')
plt.title('Mixed Trajectory Curve')
plt.xlabel('Point Index')
plt.ylabel('Position (mm)')
plt.legend()

# 绘制速度曲线
plt.subplot(1, 2, 2)
plt.plot(vx_blended, label='Vx', color='r')
plt.plot(vy_blended, label='Vy', color='g')
plt.plot(vz_blended, label='Vz', color='b')
plt.title('Mixed Velocity Curve')
plt.xlabel('Point Index')
plt.ylabel('Velocity (mm/s)')
plt.legend()

plt.tight_layout()
plt.show()



